/*********************************************************************************************************
 * File:            NavigationModule.cs
 * Description:     Provide various kinds of movement using reactive behaviors. The simple interface is
 *                  used to pass commands, and the execution interface is called automatically at regular
 *                  intervals to apply the orders via the effector components.
 ********************************************************************************************************/

using System;
using System.Collections.Generic;
using System.Text;

using XNAF = Microsoft.Xna.Framework;

using XGAE.Framework;
using XGAE.Framework.Extensions;
using xServ = XGAE.Framework.Services;
using xBody = XGAE.Framework.Body;

namespace XGAE.Abilities.Navigation
{
    /// <summary>
    /// This class contains all of the code implemented by the user.  It includes initialisation
    /// and shutdown as well as private helpers for the interface implementation.
    /// </summary>
    public class NavigationModule : Component, INavigationBase
    {
        #region Fields

        private Mode _mode; // the current state of the navigation system

        /// <summary>
        /// Tell the navigation to perform a certain task.
        /// Desired form of movement required.
        /// </summary>
        public Mode Mode
        {
            get { return this._mode; }
            set { this._mode = value; }
        }

        private XNAF.Vector3 _target; // position used for seeking and fleeing

        /// <summary>
        /// Let the system know which point in space to seek or flee.
        /// </summary>
        public XNAF.Vector3 Target
        {
            get { return this._target; }
            set { this._target = this._position + XGAE.Helpers.Multiply(this._transform, value);}
        }

        private Angles _orientation;	// estimated angles in global space

        public Angles Orientation
        {
            get { return this._orientation; }
            //set { this.orientation = value; }
        }

        private XNAF.Vector3 _position; // predicted position in world coordinates

        public XNAF.Vector3 Position
        {
            get { return this._position; }
            //set { this.position = value; }
        }

        private XNAF.Matrix _transform; // rotate from relative to local

        public XNAF.Matrix Transform
        {
            get { return this._transform; }
            //set { this.transform = value; }
        }

        private XNAF.Matrix _inverse; // transforming from local space to relative coordinates

        public XNAF.Matrix Inverse
        {
            get { return this._inverse; }
            //set { this.inverse = value; }
        }

        private int _time; // keep track of time to monitor & vary behaviors
        private float _travel; // yaw angle of the direction of movement

        public float Travel
        {
            get { return this._travel; }
            set { this._travel = value; }
        }

        #endregion

        #region C'tors

        /// <summary>
        /// Constructor calls the Base to perform automatic initialisation, then the standard 
        /// Component initialisation.  
        /// </summary>
        protected NavigationModule()
            : base("Navigation")
        {
            this._mode = Mode.Stopped;
	        this._time = 0;
	        this._position = XNAF.Vector3.Zero;
	        this._orientation = new Angles(0.0f);
	        this._travel = 0.0f;
        }

        #endregion

        #region Internal Methods

        /// <summary>
        /// Keep track of the position using odometry.
        /// Automatic polling update that should be called at around 10hz. 
        /// </summary>
        internal void Update()
        {
            // first keep track of the angular changes
            this._orientation.Incriment(this._physics.GetOrientation());
            // and clamp values to prevent problems
            for (int i = 0; i < 2; ++i)
            {
                while (this._orientation[i] > 180.0f) this._orientation[i] -= 360.0f;
                while (this._orientation[i] < -180.0f) this._orientation[i] += 360.0f;
            }
            // compute the transformation matrix accordingly
            this._transform *= XNAF.Matrix.CreateFromYawPitchRoll
                (this._orientation.Yaw, this._orientation.Pitch, this._orientation.Roll);
            //this._transform.SetAngles(this._orientation);

            // then track the movement
            XNAF.Vector3 tmp;
            this._physics.GetMovement(out tmp);
            // now update the position too
            this._position += XGAE.Helpers.Multiply(this._transform, tmp);

            // compute the inverse matrix as it is used regularly
            this._inverse = this._transform;
            XNAF.Matrix.Invert(ref this._inverse, out this._inverse);

            ++this._time;
        }

        /// <summary>
        /// Move around seemingly randomly.
        /// Use perlin noise to determine the steering force smoothly but randomly over time.
        /// </summary>
        internal void Wander()
        {
            this._travel += new Perlin(13,15731,789221,1376312589).Value( (float)_time / 10.0f ) * 15.0f;
	        this._motion.Step( XGAE.Helpers.Multiply(this._inverse, new Angles(0.0f, this._travel, 0.0f).AsVector()) );
        }

        /// <summary>
        /// Step towards the specified point.
        /// Move towards the target by computing the difference in position, and requesting the
        /// movement as a relative vector.
        /// </summary>
        internal void Seek()
        {
            XNAF.Vector3 dir = this._target - this._position;
	        dir.Z = 0;
	        float l = dir.Length();
	        if (l > 72.0f)
		        dir = dir / l;
	        else if (l > 36.0f)
		        dir = dir / 72.0f;
	        else
	        {
		        this.Mode = Mode.Stopped;
		        return;
	        }
	        this._motion.Step( XGAE.Helpers.Multiply(this._inverse, dir));
        }

        /// <summary>
        ///  Get away from a location in space.
        /// Move away from target by computing the normalised difference in position, and request the
        /// movement as a 3D vector in relative space.
        /// </summary>
        internal void Flee()
        {
            this._motion.Step
                (XGAE.Helpers.Multiply(this._inverse , XNAF.Vector3.Normalize(this._position - this._target)));
        }

        /// <summary>
        /// Determine if obstacles are in range, and avoid them!
        /// Use the visual sensors to check if there's an obstacle in the direction of travel.  This 
        /// is only possible when travel direction is within the field of view.  Otherwise, be
        /// optimistic by default; we'll use feedback from collisions steer the body away in the worst
        /// case.
        /// </summary>
        /// <returns></returns>
        internal bool Avoid()
        {
            return false;
        }

        #endregion

        #region Protected Methods

        /// <summary>
        /// Callback for Collision messages using expanded parameters.
        /// Process the message passed by the Physics component.
        /// </summary>
        /// <param name="normal"></param>
        protected void OnCollision(XNAF.Vector3 normal)
        {
            Random rnd = new Random();
            // project normal onto floor with random factors
	        XNAF.Vector3 norm = normal;
	        norm += new XNAF.Vector3
                (((float)rnd.NextDouble()*0.1f-0.05f),((float)rnd.NextDouble()*0.1f-0.05f),-norm.Z);
            norm.Normalize();
            // initialize matrix from normal with standard mirror transform
            XNAF.Matrix mirror;
            XNAF.Plane p = new XNAF.Plane(norm, 0.0f);
            XNAF.Matrix.CreateReflection(ref p, out mirror);
 	        mirror = XNAF.Matrix.Identity - mirror * 2.0f;

            // get direction of travel vector in relative space
	        XNAF.Vector3 tr = XGAE.Helpers.Multiply(this._inverse, new Angles(0.0f, this._travel, 0.0f).AsVector());

            // compute the desired orientation in relative space
            XNAF.Vector3 relative = XGAE.Helpers.Multiply(mirror, tr) + tr + normal * 0.5f;

            // set the new direction of travel as an angle
            this._travel = XGAE.Helpers.AsAngles((XGAE.Helpers.Multiply(this._transform, relative))).Yaw;
        }

        #endregion

        #region INavigationBase Implementatiom

        protected xBody.Motion.IRelative _motion;

        /// <summary>
        /// Motion hook
        /// </summary>
        public xBody.Motion.IRelative Motion
        {
            get { return this._motion; }
        }

        protected xBody.Physics.IRelative _physics;

        /// <summary>
        /// Physics hook
        /// </summary>
        public xBody.Physics.IRelative Physics
        {
            get { return this._physics; }
        }

        protected xBody.Physics.IState _state;

        /// <summary>
        /// State hook
        /// </summary>
        public xBody.Physics.IState State
        {
            get { return this._state; }
        }

        protected xBody.Vision.ISpace _space;

        /// <summary>
        /// Space hook
        /// </summary>
        public xBody.Vision.ISpace Space
        {
            get { return this._space; }
        }

        protected xBody.Vision.ISymbolic _visual;

        /// <summary>
        /// Visual hook
        /// </summary>
        public xBody.Vision.ISymbolic Visual
        {
            get { return this._visual; }
        }

        protected xServ.Debug.IVisual _debug;

        /// <summary>
        /// Debug hook
        /// </summary>
        public xServ.Debug.IVisual Debug
        {
            get { return this._debug; }
        }

        #endregion
    }
}
